/* -*- Mode:C++; c-file-style:"gnu"; indent-tabs-mode:nil; -*- */
/*
 * Copyright (c) 2019 HUST Dian Group
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation;
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 * Original Author: Pengyu Liu    <eicliupengyu@gmail.com>
 */
#include "ns3/log.h"
#include "ns3/double.h"
#include "robo-common.h"
#include "function-mobility.h"

namespace ns3 {
NS_LOG_COMPONENT_DEFINE ("FunctionMobility");
NS_OBJECT_ENSURE_REGISTERED (FunctionMobility);
FunctionMobility::FunctionMobility ()
{
  NS_LOG_FUNCTION (this);
}

FunctionMobility::~FunctionMobility ()
{
  NS_LOG_FUNCTION (this);
}

TypeId
FunctionMobility::GetTypeId (void)
{
  static TypeId tid = TypeId ("ns3::FunctionMobility")
                          .SetParent<RoboMobility> ()
                          .SetGroupName ("Robo")
                          .AddAttribute ("MaxSpeed", "Max speed", DoubleValue (M_FLOAT_MAX),
                                         MakeDoubleAccessor (&FunctionMobility::m_maxSpeed),
                                         MakeDoubleChecker<float> ())
                          .AddAttribute ("InitLocation", "Init location", DoubleValue (.5f),
                                         MakeDoubleAccessor (&FunctionMobility::m_location),
                                         MakeDoubleChecker<float> (0.f, 1.f));
  return tid;
}

TypeId
FunctionMobility::GetInstanceTypeId (void) const
{
  return GetTypeId ();
}

void
FunctionMobility::SetActor (Ptr<RoboActor> actor)
{
  NS_LOG_FUNCTION (this);
  RoboMobility::SetActor (actor);
}

void
FunctionMobility::SetFunction (LocationFunction func)
{
  m_locFunc = func;
}

void
FunctionMobility::UpdateLocation (float delta)
{
  NS_ASSERT_MSG (!m_locFunc.IsNull (), "Location Function Undefined");
  if (m_actor->m_isDestroy || m_actor->m_isDeleted)
    return;
  m_location += delta * m_speed;
  if (m_location < 0.f)
    m_location = 0.f;
  if (m_location > 1.f)
    m_location = 1.f;
  m_actor->m_globalLocation = m_locFunc (m_location);
}

void
FunctionMobility::SetSpeed (float speed)
{
  m_speed = std::min (speed, m_maxSpeed);
}

float
FunctionMobility::GetSpeed (void)
{
  return m_speed;
}

void
FunctionMobility::SetLocation (float location)
{
  m_location = location;
}

float
FunctionMobility::GetLocation (void)
{
  return m_location;
}

void
FunctionMobility::SetMaxSpeed (float speed)
{
  m_maxSpeed = speed;
}

float
FunctionMobility::GetMaxSpeed (void)
{
  return m_maxSpeed;
}

Vector
FunctionMobility::DoGetVelocity (void) const
{
  FVector speed = (m_locFunc (m_location + m_speed * 1e-3) - m_locFunc (m_location)) / 1e-3;
  return Vector (speed.m_x, speed.m_y, 0.0);
}

FVector
DefaultLocFunc (float x, float y, float phi, float loc)
{
  NS_ASSERT_MSG (loc >= 0.f && loc <= 1.f, "loc out of range");
  return ConvCoordinate (FVector (x, y), FAngle (phi), FVector (loc, 1.f - loc) * 2.828f);
}
} // namespace ns3
